package de.fhin.mas.agents.navigator.behaviours;

import jade.core.Agent;
import jade.core.behaviours.TickerBehaviour;
import de.fhin.mas.agents.navigator.NavigatorAgent;
import de.fhin.mas.util.RelativeCoordinate;

/**
 * 
 * @author Matthias Wilhelm
 * 
 */
public class Drive2 extends TickerBehaviour {
	/** Radumfang des Roboters in cm */
	private final int wheelSize;
	/** maximale Umdrehung/sec der Räder */
	private final int maxSpinPerSec;
	/** Umdrehungen pro Sekunde des linken Rades */
	private int wheelLeft;
	/** Umdrehungen pro Sekunde des rechten Rades */
	private int wheelRight;
	/** Abstand der Räder vom Mittelpunkt in cm */
	private final int axis;

	public Drive2(Agent a, long period) {
		super(a, period);
		wheelSize = 47;
		wheelLeft = 0;
		wheelRight = 0;
		maxSpinPerSec = 30;
		axis = 50;
	}

	/**
	 * 
	 */
	private static final long serialVersionUID = 6798717923212844089L;

	private void checkSpeed() {
		if (wheelLeft > maxSpinPerSec) {
			wheelLeft = maxSpinPerSec;
		}
		if (wheelRight > maxSpinPerSec) {
			wheelRight = maxSpinPerSec;
		}
	}

	private void speed(int distance, int angle) {
		int leftToRight = (angle / distance);
		wheelLeft = (distance / 3) * leftToRight;
		wheelRight = (distance / 3) / (1 / leftToRight);
	}

	@Override
	protected void onTick() {
		RelativeCoordinate nextMove = ((NavigatorAgent) myAgent).get_nextMove();
		int angle = nextMove.getAngle();
		int distance = nextMove.getDistance();

		if (angle == 0) {
			wheelLeft = distance / 3;
			wheelRight = distance / 3;
		} else if (distance == 0) {
			if (angle > 0) {
				wheelLeft = 1;
				wheelRight = -1;
			} else {
				wheelLeft = -1;
				wheelRight = 1;
			}
		} else {
			speed(angle, distance);
		}
		// Zur jetzigen Position die neue angefahrene Position berechnen und
		// addieren

		System.out.println("Speed left wheel: " + wheelLeft
				+ ". Speed right wheel: " + wheelRight + ".");
		int distanceLeft = wheelSize * wheelLeft;
		int distanceRight = wheelSize * wheelRight;

		if (((NavigatorAgent) myAgent).get_myPosition().distance(
				((NavigatorAgent) myAgent).get_myDestination()) < 3) {
			((NavigatorAgent) myAgent).target_reached();
		}
	}
}
